/*
 * camera.c
 *
 *  Created on: 2024�?6�?6�?
 *      Author: Merlin_yang
 */

#include "zf_device_mt9v03x.h"
#include "zf_device_ips200.h"
#include "zf_common_debug.h"
#include "zf_common_font.h"
#include "zf_driver_gpio.h"
#include "leg_control.h"
#include "camera.h"
#include "open_count.h"
#include "pid.h"
#include "spi_wifi.h"
#include "stdbool.h"
#include "open_count.h"
#include "cpu0init.h"
#include "centerline_generator.h"
float turn_angle = 0;
uint8 stop_flag = 2;
float h = 0;  // 全局化的h变量

volatile int time = 0;
Point left_edge[IMAGE_HEIGHT * 2];
Point right_edge[IMAGE_HEIGHT * 2];
Point center_points[IMAGE_HEIGHT * 2];
Point left_world_pts[IMAGE_HEIGHT * 2];
Point right_world_pts[IMAGE_HEIGHT * 2];
Point left_resample_pts[IMAGE_HEIGHT * 2];
Point right_resample_pts[IMAGE_HEIGHT * 2];
Point center_resample_pts[IMAGE_HEIGHT * 2];
Point world_zebra_point[2] = {{525, 700}, {750, 700}};
Point screen_zebra_point[2] = {{0, 0}, {0, 0}};
Point left_zebra = {0,0}, right_zebra = {0,0};
int left_count = 0, right_count = 0, center_count = 0, left_count_w = 0, right_count_w = 0, left_count_resample = 0, right_count_resample = 0, center_count_resample = 0;
float turning_radius_m = 0;

int const_C_bright = 18;
int const_C_dark = -15;
int brightness_split_point = 140;
int ABSOLUTE_BLACK_THRESHOLD = 103;
int real_zebra_distance = 0;

extern CenterlineGenerator g_centerline_gen;
static unsigned char visited[IMAGE_HEIGHT][IMAGE_WIDTH];
IFX_ALIGN(4) uint8  original_image[MT9V03X_H][MT9V03X_W];
IFX_ALIGN(4) uint8  binary_image[MT9V03X_H][MT9V03X_W];

struct PID_parameters pid_turn_angle =
{
  .kp = 0.5,
  .ki = 0,
  .kd = 0.2,
  .max_kp_product = 3000,
  .max_ki_product = 0,
  .max_kd_product = 3000,
  .max_pid_sum = 3000,
};


void camera_mt9v03x_init(void)
    {
        zf_log(!mt9v03x_init(), "mt9v03x init error");
    }

void save_image(void)
{
    memcpy(original_image[0], mt9v03x_image[0], MT9V03X_IMAGE_SIZE);
}

/**
 * @brief 使用基于降采样的快速局部自适应阈值算法对图像进行二值化
 * @param src_image  输入的灰度图像 (通常是高斯模糊后的)
 * @param dst_image  输出的二值化图像
 * @note [极限性能优化] 此版本通过在原图1/4大小的缩略图上进行耗时的自适应阈值计算，
 * 从根本上解决了性能瓶颈。预计可将此步骤耗时降低75%。
 */
void adaptive_thresholding_fast(const unsigned char src_image[IMAGE_HEIGHT][IMAGE_WIDTH], unsigned char dst_image[IMAGE_HEIGHT][IMAGE_WIDTH])
{
    // --- 静态缓冲区，用于存储小图像、积分图和【新增】的阈值图 ---
    static unsigned char small_src[SMALL_IMAGE_HEIGHT][SMALL_IMAGE_WIDTH];
    static unsigned int integral_image[SMALL_IMAGE_HEIGHT][SMALL_IMAGE_WIDTH];
    static unsigned char small_threshold_map[SMALL_IMAGE_HEIGHT][SMALL_IMAGE_WIDTH]; // 新增的阈值查找表

    // --- 可调参数 ---
    const int block_size = 11;

    // --- 步骤 1: 图像降采样 (不变) ---
    for (int y = 0; y < SMALL_IMAGE_HEIGHT; ++y) {
        for (int x = 0; x < SMALL_IMAGE_WIDTH; ++x) {
            int sum = src_image[y * 2][x * 2] + src_image[y * 2 + 1][x * 2] + src_image[y * 2][x * 2 + 1] + src_image[y * 2 + 1][x * 2 + 1];
            small_src[y][x] = (unsigned char)(sum >> 2);
        }
    }

    // --- 步骤 2: 在小图上计算积分图像 (不变) ---
    integral_image[0][0] = small_src[0][0];
    for (int x = 1; x < SMALL_IMAGE_WIDTH; ++x) {
        integral_image[0][x] = integral_image[0][x - 1] + small_src[0][x];
    }
    for (int y = 1; y < SMALL_IMAGE_HEIGHT; ++y) {
        unsigned int row_sum = 0;
        for (int x = 0; x < SMALL_IMAGE_WIDTH; ++x) {
            row_sum += small_src[y][x];
            integral_image[y][x] = integral_image[y - 1][x] + row_sum;
        }
    }

    // --- 步骤 3: 【性能优化版】在小图上计算并存储“阈值查找表” ---
    const int half_block = block_size / 2;
    const int core_width = SMALL_IMAGE_WIDTH - block_size;
    const int core_height = SMALL_IMAGE_HEIGHT - block_size;
    const unsigned int core_area = block_size * block_size;

    // 遍历所有行
    for (int y = 0; y < SMALL_IMAGE_HEIGHT; ++y) {
    // 预计算y方向的边界
    int y1 = y - half_block;
    int y2 = y + half_block;
    // 钳位y坐标
    if (y1 < 0) y1 = 0;
    if (y2 >= SMALL_IMAGE_HEIGHT) y2 = SMALL_IMAGE_HEIGHT - 1;

    // 遍历所有列
    for (int x = 0; x < SMALL_IMAGE_WIDTH; ++x) {
        int x1 = x - half_block;
        int x2 = x + half_block;
        unsigned int sum, count, mean;

        // 判断是否在核心区域，利用短路求值优化判断
        if (x > half_block && y > half_block && x < core_width && y < core_height) {
            // --- 核心区域：无边界检查，最高效 ---
            // 此时 x1, y1 > 0 且 x2, y2 < 图像边界
            // 积分图计算公式直接应用
            sum = integral_image[y2][x2]
                - integral_image[y1 - 1][x2]
                - integral_image[y2][x1 - 1]
                + integral_image[y1 - 1][x1 - 1];
            count = core_area; // 面积是常量
        } else {
            // --- 边界区域：执行原始的边界检查和计算 ---
            if (x1 < 0) x1 = 0;
            if (x2 >= SMALL_IMAGE_WIDTH) x2 = SMALL_IMAGE_WIDTH - 1;
            // y1, y2 已经在外层循环钳位过

            unsigned int top_left = (x1 > 0 && y1 > 0) ? integral_image[y1 - 1][x1 - 1] : 0;
            unsigned int top_right = (y1 > 0) ? integral_image[y1 - 1][x2] : 0;
            unsigned int bottom_left = (x1 > 0) ? integral_image[y2][x1 - 1] : 0;
            
            sum = integral_image[y2][x2] - top_right - bottom_left + top_left;
            count = (y2 - y1 + 1) * (x2 - x1 + 1); // 面积需要计算
        }

        // --- 后续计算保持不变 ---
        // 避免除以零的错误
        if (count == 0) {
            mean = small_src[y][x]; // 或其他合理默认值
        } else {
            mean = sum / count;
        }

        int const_C = (mean > brightness_split_point) ? const_C_bright : const_C_dark;

        int threshold = mean - const_C;

        // 钳位操作 (Clipping)
        if (threshold < 0) threshold = 0;
        if (threshold > 255) threshold = 255;

        small_threshold_map[y][x] = (unsigned char)threshold;
        }
    }

    // --- 步骤 4: 【核心修改】在全尺寸原图上，应用查找表进行二值化 ---
    for (int Y = 0; Y < IMAGE_HEIGHT; ++Y) {
        for (int X = 0; X < IMAGE_WIDTH; ++X) {
            // 同样应用绝对黑色阈值，提高效率
            if (src_image[Y][X] < ABSOLUTE_BLACK_THRESHOLD) {
                dst_image[Y][X] = PIXEL_BACKGROUND;
                continue;
            }

            // 1. 找到当前大图像素(X, Y)对应的小图坐标(x, y)
            int small_x = X >> 1;
            int small_y = Y >> 1;

            // 2. 从阈值查找表中取出预先算好的局部阈值
            unsigned char threshold = small_threshold_map[small_y][small_x];

            // 3. 在全分辨率上进行比较和二值化
            if (src_image[Y][X] > threshold) {
                dst_image[Y][X] = PIXEL_TRACK;
            } else {
                dst_image[Y][X] = PIXEL_BACKGROUND;
            }
        }
    }
}

int find_longest_vertical_run_from_row(unsigned char image[IMAGE_HEIGHT][IMAGE_WIDTH], int start_row, int tolerance) {
    // 参数有效性检查
    if (start_row < 0 || start_row >= IMAGE_HEIGHT) {
        return -1; // 起始行越界
    }

    int max_length = 0;
    int best_column_index = -1;

    // 1. 逐列进行考察
    for (int col = 0; col < IMAGE_WIDTH; ++col) {
        int current_length = 0;
        int tolerance_count = 0;

        // 2. 对于当前列，从指定的 start_row 开始，垂直向下扫描
        for (int row = start_row; row > 0; --row) {
            if (image[row][col] == 255) {
                // 是白色像素，增加长度
                current_length++;
                tolerance_count = 0; // 重置容忍度
            } else {
                // 是黑色像素，增加容忍度计数
                tolerance_count++;
                if (tolerance_count > tolerance) {
                    // 超过容忍度，中断该列的扫描
                    break; // 跳出内层循环，考察下一列
                }
            }
        }

        // 3. 一列扫描结束后，比较其长度是否为当前最长
        if (current_length > max_length) {
            max_length = current_length;
            best_column_index = col;
        }
    }

    return best_column_index;
}

/**
 * @brief 内部辅助函数，用于沿单条边线进行轮廓追踪 (最终修复版)
 * @note 修复了左右边线旋转方向不一致的问题。
 * 右边线采用逆时针(CCW)追踪，左边线采用顺时针(CW)追踪，
 * 确保两者都能从底部正确地向上搜索。
 */
static void crawl_single_edge(
        const unsigned char binary_img[IMAGE_HEIGHT][IMAGE_WIDTH],
        Point start_point,
        EdgeType edge_type,
        int initial_search_direction_idx,
        bool starts_on_boundary, // 接收“出身”信息, // 使用传入的初始方向
        Point edge_points[],
        int* point_count,
        unsigned char visited[IMAGE_HEIGHT][IMAGE_WIDTH]
) {
    Point current_p = start_point;
    int search_direction_idx = initial_search_direction_idx;

    const int dx[] = {1, 1, 0, -1, -1, -1, 0, 1}; // CCW
    const int dy[] = {0, -1, -1, -1, 0, 1, 1, 1};
    int step = (edge_type == EDGE_RIGHT) ? 1 : -1;
    // [新功能：垂直边界限制] 初始化计数器和状态
    int vertical_streak_count = 0;
    bool is_in_vertical_streak = starts_on_boundary;
    int point_count_switch = 120;
    if (g_centerline_gen.current_state == STATE_BRIDGE_HANDLING) point_count_switch = 240;
    else point_count_switch = 120;
    while (*point_count < point_count_switch) {
        if (current_p.y < 0 || current_p.y >= IMAGE_HEIGHT ||
            current_p.x < 0 || current_p.x >= IMAGE_WIDTH) { //  || visited[current_p.y][current_p.x]
            break;
        }

        if (is_in_vertical_streak) {
            if (abs(current_p.x - start_point.x) <= 3) {
                vertical_streak_count++;
                if (vertical_streak_count >= 52) { // 使用你设定的52点
                    break; // 直接break，不再记录点
                }
            } else {
                is_in_vertical_streak = false;
            }
        }

        // 所有检查通过后，才正式记录当前点
        edge_points[*point_count] = current_p;
        (*point_count)++;
        visited[current_p.y][current_p.x] = 1;

        bool found_next_point = false;
        bool terminate_crawl = false;
        for (int i = 0; i < 8; i++) {
            int search_idx = (search_direction_idx + i * step + 8) % 8;
            Point next_p = {current_p.x + dx[search_idx], current_p.y + dy[search_idx]};

            // 正常的循线不应该越过边界
            if (next_p.x < 0 || next_p.x >= IMAGE_WIDTH || next_p.y < 0 || next_p.y >= IMAGE_HEIGHT) {
                continue;
            }

            // [新策略] 如果不是从边界启动，但下一步要碰到边界了，则忽略该点
            if (!starts_on_boundary) {
                if (next_p.x == 0 || next_p.x == IMAGE_WIDTH - 1 ||
                    next_p.y == 0 || next_p.y == IMAGE_HEIGHT - 1) {
                    terminate_crawl = true;
                    break; // 将此点视为非法，继续搜索其他邻域点
                }
            }
            if (next_p.y == IMAGE_HEIGHT - 1) continue;

            if (binary_img[next_p.y][next_p.x] == PIXEL_TRACK) {
                int prev_neighbor_idx = (search_idx - step + 8) & 7;
                Point bg_check_p = {current_p.x + dx[prev_neighbor_idx], current_p.y + dy[prev_neighbor_idx]};

                bool is_behind_background = false;
                // [最终修正] 核心逻辑
                if (bg_check_p.x < 0 || bg_check_p.x >= IMAGE_WIDTH || bg_check_p.y < 0) {
                    // 如果“身后”的点在图像外
                    // 只有当起始点就在边界时，才承认这是合法的“墙”
                    if (starts_on_boundary) {
                        is_behind_background = true;
                    }
                } else {
                    // 如果“身后”的点在图像内，正常判断
                    if (binary_img[bg_check_p.y][bg_check_p.x] == PIXEL_BACKGROUND) {
                        is_behind_background = true;
                    }
                }

                if (is_behind_background) {
                    current_p = next_p;

                    // 如果我们刚刚是沿着直线（向上）移动的
                    if (search_idx == 2) {
                        search_direction_idx = 2; // 下一次继续优先向上
                    } else {
                        search_direction_idx = prev_neighbor_idx; // 在拐弯时，才用“墙”的方向做引导
                    }

                    found_next_point = true;
                    break;
                }
            }
        }

        // [逻辑修正] 检查终止标志位
        if (terminate_crawl) {
            break; // 跳出 while 循环
        }

        if (!found_next_point) {
            break;
        }
    }
}

/**
 * @brief 使用八邻域爬线算法检测二值图像中的边线
 * @param binary_img 输入的二值化图像
 * @param left_edge_points 输出的左边线点数组
 * @param right_edge_points 输出的右边线点数组
 * @param left_count 输出的左边线点数量
 * @param right_count 输出的右边线点数量
 * @note 从图像底部寻找起始点，然后使用八邻域爬线算法追踪边线。
 * 此版本修复了static数组滥用、边界判断漏洞和代码冗余问题。
 */
void detect_edges_eight_neighbor(
    const unsigned char binary_img[IMAGE_HEIGHT][IMAGE_WIDTH],
    Point left_edge_points[], Point right_edge_points[],
    int* left_count, int* right_count
) {
    *left_count = 0;
    *right_count = 0;

    // 清零全局visited数组
    memset(visited, 0, sizeof(visited));

    Point left_start_p = {-1, -1}, right_start_p = {-1, -1};
    int start_y = IMAGE_HEIGHT - START_SCAN_OFFSET_FROM_BOTTOM;
    if (g_centerline_gen.current_state == STATE_INTERSECTION_HANDLING) start_y = IMAGE_HEIGHT - kanyuan;
    // [更新3] 边界标志位现在是局部变量
    bool is_left_on_boundary = false;
    bool is_right_on_boundary = false;
    int search_center = g_last_frame_center_x;
    int search_center_left = 0;
    int search_center_right = 0;
    if (g_centerline_gen.current_state == STATE_INTERSECTION_HANDLING ) {
        search_center = find_longest_vertical_run_from_row(binary_img, IMAGE_HEIGHT - START_SCAN_OFFSET_FROM_BOTTOM, 2);
        search_center_left = search_center;
        search_center_right = search_center;
    }else{
        search_center_left = search_center - 6;
        search_center_right = search_center - 6;
    }
    // --- 步骤2: 初始搜索 (寻找起始点) ---
    // 搜索左边线起点：从中心向左完整扫描
    for (int x = search_center_left; x > 5; --x) {
        if (binary_img[start_y][x] == PIXEL_TRACK && binary_img[start_y][x - 1] == PIXEL_BACKGROUND) {
            left_start_p = (Point){x, start_y};
            break;
        }
    }
    // [更新2] 边界检测条件强化为3个点
    if (left_start_p.x == -1 &&
        binary_img[start_y][0] == PIXEL_TRACK &&
        binary_img[start_y][1] == PIXEL_TRACK &&
        binary_img[start_y][2] == PIXEL_TRACK &&
        !g_centerline_gen.is_left_corner_locked) {
        left_start_p = (Point){0, start_y};
        is_left_on_boundary = true;
    }

    // 搜索右边线起点：从中心向右完整扫描
    for (int x = search_center_right; x < IMAGE_WIDTH - 5; ++x) {
        if (binary_img[start_y][x] == PIXEL_TRACK && binary_img[start_y][x + 1] == PIXEL_BACKGROUND) {
            right_start_p = (Point){x, start_y};
            break;
        }
    }
    // [更新2] 边界检测条件强化为3个点
    if (right_start_p.x == -1 &&
        binary_img[start_y][IMAGE_WIDTH - 2] == PIXEL_TRACK &&
        binary_img[start_y][IMAGE_WIDTH - 3] == PIXEL_TRACK &&
        binary_img[start_y][IMAGE_WIDTH - 4] == PIXEL_TRACK &&
        !g_centerline_gen.is_right_corner_locked) {
        right_start_p = (Point){IMAGE_WIDTH - 1, start_y};
        is_right_on_boundary = true;
    }
    if (!is_left_on_boundary || !is_right_on_boundary){
        // --- 步骤3: 从起点开始爬线 ---
        if (left_start_p.x != -1) {
            int initial_dir;
            if (is_left_on_boundary) {
                initial_dir = 2; // [最终方案] 左边界起步，优先向上
            } else {
                initial_dir = 3; // [最终方案] 常规左边线，优先向左上
            }
            crawl_single_edge(binary_img, left_start_p, EDGE_LEFT, initial_dir, is_left_on_boundary, left_edge_points, left_count, visited);
        }

        if (right_start_p.x != -1) {
            int initial_dir;
            if (is_right_on_boundary) {
                initial_dir = 2; // [最终方案] 右边界起步，优先向上
            } else {
                initial_dir = 1; // [最终方案] 常规右边线，优先向右上
            }
            crawl_single_edge(binary_img, right_start_p, EDGE_RIGHT, initial_dir, is_right_on_boundary, right_edge_points, right_count, visited);
        }
    }

    // --- 步骤4: 更新全局状态 ---
    // 仅在左右边线都找到时，才更新中心点位置
    if (*left_count > 0 && *right_count > 0) {
        int bottom_left_x = left_edge_points[0].x;
        int bottom_right_x = right_edge_points[0].x;
        int current_center = (bottom_left_x + bottom_right_x) / 2;
        // 使用低通滤波平滑更新，避免突变
        g_last_frame_center_x = (int)(g_last_frame_center_x * 0.5 + current_center * 0.5);
    }
}

/**
 * @brief 在IPS200屏幕上显示边线点（支持多实例版本）
 * @param state 显示状态结构体指针，用于存储每个实例的状态
 * @param left_edge 左边线点数组
 * @param right_edge 右边线点数组
 * @param left_count 左边线点数量
 * @param right_count 右边线点数量
 * @param left_color 左边线颜色
 * @param right_color 右边线颜色
 */
void display_edge_points_on_ips200_xy_multi(
    DisplayState* state,
    const Point left_edge[],
    const Point right_edge[],
    int left_count,
    int right_count,
    uint16_t left_color,
    uint16_t right_color
) {
    // 如果不是第一帧，先用黑色擦除上一帧的点
    if (!state->first_frame) {
        // 无条件擦除上一帧的左边线点
        for (int i = 0; i < state->prev_left_count; i++) {
            if (state->prev_left_edge[i].x >= 0 && state->prev_left_edge[i].x < 240 &&
                state->prev_left_edge[i].y >= 0 && state->prev_left_edge[i].y < 240) {
                ips200_draw_point((int)(state->prev_left_edge[i].x), (int)(state->prev_left_edge[i].y), RGB565_BLACK);
            }
        }
        // 无条件擦除上一帧的右边线点
        for (int i = 0; i < state->prev_right_count; i++) {
            if (state->prev_right_edge[i].x >= 0 && state->prev_right_edge[i].x < 240 &&
                state->prev_right_edge[i].y >= 0 && state->prev_right_edge[i].y < 240) {
                ips200_draw_point((int)(state->prev_right_edge[i].x), (int)(state->prev_right_edge[i].y), RGB565_BLACK);
            }
        }
    }

    // 显示当前帧的左边线点（如果不为NULL）
    if (left_edge != NULL) {
        for (int i = 0; i < left_count; i++) {
            // 无条件保存点到prev数组，确保擦除时能找到
            state->prev_left_edge[i] = left_edge[i];
            // 只有在边界内才绘制点
            if (left_edge[i].x >= 0 && left_edge[i].x < 240 && left_edge[i].y >= 0 && left_edge[i].y < 240) {
                ips200_draw_point((int)(left_edge[i].x), (int)(left_edge[i].y), left_color);
            }
        }
        state->prev_left_count = left_count;
    } else {
        // 如果左边线为NULL，重置计数
        state->prev_left_count = 0;
    }

    // 显示当前帧的右边线点（如果不为NULL）
    if (right_edge != NULL) {
        for (int i = 0; i < right_count; i++) {
            // 无条件保存点到prev数组，确保擦除时能找到
            state->prev_right_edge[i] = right_edge[i];
            // 只有在边界内才绘制点
            if (right_edge[i].x >= 0 && right_edge[i].x < 240 && right_edge[i].y >= 0 && right_edge[i].y < 240) {
                ips200_draw_point((int)(right_edge[i].x), (int)(right_edge[i].y), right_color);
            }
        }
        state->prev_right_count = right_count;
    } else {
        // 如果右边线为NULL，重置计数
        state->prev_right_count = 0;
    }

    // 标记已经不是第一帧
    state->first_frame = 0;
}

#define MAX_POINTS 240
/**
 * @brief 生成一维高斯核
 * * @param kernel          [out] 指向用于存储高斯核的浮点数数组的指针
 * @param kernel_size     [in]  高斯核的大小 (必须是正奇数, e.g., 3, 5, 7)
 * @param sigma           [in]  高斯分布的标准差 (e.g., 1.0, 1.5)
 * * @note 该函数在系统初始化时调用一次即可，无需在主循环中反复计算。
 */
void generate_gaussian_kernel(float* kernel, int kernel_size, float sigma) {
    // 确保窗口大小为奇数
    if (kernel_size % 2 == 0) {
        // 在实际工程中，这里应该有错误处理，例如返回错误码或断言
        // for example: return -1;
        kernel_size++; // 简单处理：强制变为奇数
    }

    float sum = 0.0f;
    int radius = kernel_size / 2;

    // 计算高斯核的每一个权重值
    for (int i = 0; i < kernel_size; i++) {
        int x = i - radius;
        // 高斯函数公式: G(x) = (1 / (sqrt(2*PI) * sigma)) * exp(-(x^2) / (2 * sigma^2))
        // 为了简化和提高效率，我们先不计算前面的常数部分，最后再进行归一化
        kernel[i] = exp(-(x * x) / (2 * sigma * sigma));
        sum += kernel[i];
    }

    // 归一化，确保所有权重之和为1
    for (int i = 0; i < kernel_size; i++) {
        kernel[i] /= sum;
    }
}

/**
 * @brief (Point结构体版) 对一维路径数据应用高斯滤波。
 *
 * @param input_path      [in]  指向输入路径数组(Point类型)的指针。
 * @param output_path     [out] 指向输出路径数组(Point类型)的指针。
 * @param data_size       [in]  路径点的数量。
 * @param kernel          [in]  指向已生成的一维高斯核数组的指针。
 * @param kernel_size     [in]  高斯核的大小。
 */
void gaussian_filter_path(const Point* input_path, Point* output_path, int data_size, const float* kernel, int kernel_size) {
    int radius = kernel_size / 2;
    int i = 0;

    // --- 1. 处理左边界 (内核左侧超出范围) ---
    for (i = 0; i < radius && i < data_size; i++) {
        float sum_x = 0.0f, sum_y = 0.0f;
        for (int j = 0; j < kernel_size; j++) {
            int sample_index = i + j - radius;
            // 边界处理：只处理 sample_index < 0 的情况
            if (sample_index < 0) {
                sample_index = 0;
            }
            sum_x += input_path[sample_index].x * kernel[j];
            sum_y += input_path[sample_index].y * kernel[j];
        }
        output_path[i].x = (int)(sum_x + 0.5f);
        output_path[i].y = (int)(sum_y + 0.5f);
    }

    // --- 2. 处理中间主体 (内核完全在数据内部，无须边界判断) ---
        // 这是函数执行的主要部分，性能提升最显著
    for (; i < data_size - radius; i++) {
        float sum_x = 0.0f, sum_y = 0.0f;
        int j = 0;
        // 假设 kernel_size 是奇数，比如7。我们可以展开 (7-1)/2=3 次
        for (j = 0; j < kernel_size - 1; j += 2) {
            int sample_index1 = i + j - radius;
            int sample_index2 = i + j + 1 - radius;

            sum_x += input_path[sample_index1].x * kernel[j] + input_path[sample_index2].x * kernel[j+1];
            sum_y += input_path[sample_index1].y * kernel[j] + input_path[sample_index2].y * kernel[j+1];
        }
        // 处理剩余的单次迭代 (如果kernel_size是奇数)
        sum_x += input_path[i + j - radius].x * kernel[j];
        sum_y += input_path[i + j - radius].y * kernel[j];

        output_path[i].x = (int)(sum_x + 0.5f);
        output_path[i].y = (int)(sum_y + 0.5f);
    }

    // --- 3. 处理右边界 (内核右侧超出范围) ---
    for (; i < data_size; i++) {
        float sum_x = 0.0f, sum_y = 0.0f;
        for (int j = 0; j < kernel_size; j++) {
            int sample_index = i + j - radius;
            // 边界处理：只处理 sample_index >= data_size 的情况
            if (sample_index >= data_size) {
                sample_index = data_size - 1;
            }
            sum_x += input_path[sample_index].x * kernel[j];
            sum_y += input_path[sample_index].y * kernel[j];
        }
        output_path[i].x = (int)(sum_x + 0.5f);
        output_path[i].y = (int)(sum_y + 0.5f);
    }
}

/**
 * @brief (整型坐标版) 对输入的鸟瞰图路径进行平滑和等距重采样。
 *
 * @param path_in           (输入) 原始的、带有噪声的鸟瞰图路径 (整型坐标)。
 * @param point_count_in    (输入) 原始路径点的数量。
 * @param path_out          (输出) 用于存储最终处理后路径的缓冲区。
 * @param point_count_out   (输出) 指向一个整数的指针，用于返回最终生成的路径点数量。
 * @param window_size       (输入) 滑动平均滤波的窗口大小
 * @param step_distance     (输入) 重采样的物理步长
 */
void process_path_int(
    const Point* path_in,
    uint8_t point_count_in,
    Point* path_out,
    uint8_t* point_count_out,
    const float* gaussian_kernel,
    int kernel_size,
    float step_distance
)
{
    // --- 步骤1: 高斯滤波 (不变) ---
    Point smoothed_path[240];
    gaussian_filter_path(path_in, smoothed_path, point_count_in, gaussian_kernel, kernel_size);

    // --- 步骤2: 等距重采样 (修正后的优化版本) ---
    *point_count_out = 0;

    if (point_count_in < 2) {
        if (point_count_in == 1) {
            path_out[0] = smoothed_path[0];
            *point_count_out = 1;
        }
        return;
    }

    // 初始化输出路径的第一个点
    path_out[0] = smoothed_path[0];
    *point_count_out = 1;
    
    // 这个变量追踪从上一个被放置的点开始，还需要走多远才能放置下一个点。
    float dist_to_next_point = step_distance;

    for (int i = 1; i < point_count_in; ++i) {
        Point p_start = smoothed_path[i - 1];
        Point p_end = smoothed_path[i];

        float dx = (float)(p_end.x - p_start.x);
        float dy = (float)(p_end.y - p_start.y);
        float segment_length = sqrtf(dx * dx + dy * dy);

        // <--- 核心修正逻辑在这里 ---
        // 只要当前线段的长度大于我们需要的距离，就在这个线段上放置新点
        while (segment_length >= dist_to_next_point) {
            if (*point_count_out >= MAX_POINTS) return;

            // 计算新点的位置
            // ratio是需要的距离在当前线段总长中的比例
            float ratio = dist_to_next_point / segment_length;

            float new_x = (float)p_start.x + dx * ratio;
            float new_y = (float)p_start.y + dy * ratio;
            
            Point new_point = {(int)(new_x + 0.5f), (int)(new_y + 0.5f)};
            path_out[*point_count_out] = new_point;
            (*point_count_out)++;

            // 更新状态，准备放置下一个点
            // 1. 我们放置了一个点，所以线段的起点现在是这个新点
            p_start = new_point;
            // 2. 新的dx, dy是终点相对于新起点的
            dx = (float)(p_end.x - p_start.x);
            dy = (float)(p_end.y - p_start.y);
            // 3. 线段的剩余长度可以用减法得到，避免了sqrtf！
            segment_length -= dist_to_next_point;
            // 4. 重置下一个目标点需要的距离
            dist_to_next_point = step_distance;
        }

        // 当前线段处理完毕，它的剩余长度不足以放置一个新点。
        // 我们将这个剩余长度从下一个目标距离中减去，
        // 这样下一个 for 循环就会带着这个“欠账”去处理下一条线段。
        dist_to_next_point -= segment_length;
    }
}
